#include "caveViewer.h"
#include <osg/PositionAttitudeTransform>
#include <osg/MatrixTransform>
#include <osg/Light>
#include <osg/Material>
#include <osgGA/TrackballManipulator>
#include <osg/Uniform>
#include <iostream>

#include "ApplyShaderToTexturedGeodeVisitor.h"

#define CYCLOPE_ID 0
#define FLYSTICK_ID 5

//-------------------------------------------------------------------------------------------------------

static float ClampUnity(float x)
{
   if (x >  1.0f) { return  1.0f; }
   if (x < -1.0f) { return -1.0f; }
   return x;
}

static void HprToMatrix( osg::Matrix& rotation, const osg::Vec3& hpr )
{
   // implementation converted from plib's sg.cxx
   // PLIB - A Suite of Portable Game Libraries
   // Copyright (C) 1998,2002  Steve Baker
   // For further information visit http://plib.sourceforge.net


   double ch, sh, cp, sp, cr, sr, srsp, crsp, srcp;

   // this can't be smart for both 32 and 64 bit types.
   ///\todo find a preprocessor way to assign this constant different for the different precision types.
   const osg::Vec3::value_type magic_epsilon = (osg::Vec3::value_type)0.00001;

   if (osg::equivalent(hpr[0], (osg::Vec3::value_type)0.0, magic_epsilon))
   {
      ch = 1.0;
      sh = 0.0;
   }
   else
   {
      sh = sinf(osg::DegreesToRadians(hpr[0]));
      ch = cosf(osg::DegreesToRadians(hpr[0]));
   }

   if (osg::equivalent(hpr[1], (osg::Vec3::value_type)0.0, magic_epsilon))
   {
      cp = 1.0;
      sp = 0.0;
   }
   else
   {
      sp = sinf(osg::DegreesToRadians(hpr[1]));
      cp = cosf(osg::DegreesToRadians(hpr[1]));
   }

   if (osg::equivalent(hpr[2], (osg::Vec3::value_type)0.0, magic_epsilon))
   {
      cr   = 1.0;
      sr   = 0.0;
      srsp = 0.0;
      srcp = 0.0;
      crsp = sp;
   }
   else
   {
      sr   = sinf(osg::DegreesToRadians(hpr[2]));
      cr   = cosf(osg::DegreesToRadians(hpr[2]));
      srsp = sr * sp;
      crsp = cr * sp;
      srcp = sr * cp;
   }

   rotation(0, 0) =  ch * cr - sh * srsp;
   rotation(1, 0) = -sh * cp;
   rotation(2, 0) =  sr * ch + sh * crsp;

   rotation(0, 1) =  cr * sh + srsp * ch;
   rotation(1, 1) =  ch * cp;
   rotation(2, 1) =  sr * sh - crsp * ch;

   rotation(0, 2) = -srcp;
   rotation(1, 2) =  sp;
   rotation(2, 2) =  cr * cp;

   rotation(3, 0) =  0.0;  // x trans
   rotation(3, 1) =  0.0;  // y trans
   rotation(3, 2) =  0.0;  // z trans

   rotation(0, 3) =  0.0;
   rotation(1, 3) =  0.0;
   rotation(2, 3) =  0.0;
   rotation(3, 3) =  1.0;
}

static void MatrixToHpr( osg::Vec3& hpr, const osg::Matrix& rotation )
{
   // implementation converted from plib's sg.cxx
   // PLIB - A Suite of Portable Game Libraries
   // Copyright (C) 1998,2002  Steve Baker
   // For further information visit http://plib.sourceforge.net

   osg::Matrix mat;

   osg::Vec3 col1(rotation(0, 0), rotation(0, 1), rotation(0, 2));
   double s = col1.length();

   const double magic_epsilon = 0.00001;
   if (s <= magic_epsilon)
   {
      hpr.set(0.0f, 0.0f, 0.0f);
      return;
   }


   double oneOverS = 1.0f / s;
   for (int i = 0; i < 3; ++i)
   {
      for (int j = 0; j < 3; ++j)
      {
         mat(i, j) = rotation(i, j) * oneOverS;
      }
   }


   double sin_pitch = ClampUnity(mat(1, 2));
   double pitch = asin(sin_pitch);
   hpr[1] = osg::RadiansToDegrees(pitch);

   double cp = cos(pitch);

   if (cp > -magic_epsilon && cp < magic_epsilon)
   {
      double cr = ClampUnity(-mat(2,1));
      double sr = ClampUnity(mat(0,1));

      hpr[0] = 0.0f;
      hpr[2] = osg::RadiansToDegrees(atan2(sr,cr));
   }
   else
   {
      double one_over_cp = 1.0 / cp;
      double sr = ClampUnity(-mat(0,2) * one_over_cp);
      double cr = ClampUnity( mat(2,2) * one_over_cp);
      double sh = ClampUnity(-mat(1,0) * one_over_cp);
      double ch = ClampUnity( mat(1,1) * one_over_cp); 

      if ((osg::equivalent(sh,0.0,magic_epsilon) && osg::equivalent(ch,0.0,magic_epsilon)) ||
          (osg::equivalent(sr,0.0,magic_epsilon) && osg::equivalent(cr,0.0,magic_epsilon)) )
      {
         cr = ClampUnity(-mat(2,1));
         sr = ClampUnity(mat(0,1));;

         hpr[0] = 0.0f;
      }
      else
      {
        hpr[0] = osg::RadiansToDegrees(atan2(sh, ch));
      }

      hpr[2] = osg::RadiansToDegrees(atan2(sr, cr));
   }
}

//-------------------------------------------------------------------------------------------------------

CCaveViewer::CCaveViewer()
{
	m_leftView = NULL;
	m_rightView = NULL;

	m_eyeDist = 0.065;
	m_shiftWidth = 60;
	m_caveFaceId = CAVE_CENTER;
	m_startEye = osg::Vec3(0, 0, 0);
	m_caveWidth = m_caveDepth = 3.0;
	m_caveHeight = 2.7;
	m_caveGroundTop = m_caveGroundBottom = 3.0;
	m_sceneScale = osg::Vec3(1, 1, 1);
}

CCaveViewer::~CCaveViewer()
{
}

void CCaveViewer::init()
{

	// set window parameters
	osg::GraphicsContext::WindowingSystemInterface* wsi = osg::GraphicsContext::getWindowingSystemInterface();
	if(!wsi)
		return;
	unsigned int windowWidth, windowHeight;
	wsi->getScreenResolution(osg::GraphicsContext::ScreenIdentifier(0), windowWidth, windowHeight);

	osg::ref_ptr<osg::GraphicsContext::Traits> traits = new osg::GraphicsContext::Traits;
	traits->x = 0;
	traits->y = 0;
	traits->width = windowWidth;
	traits->height = windowHeight;
	traits->windowDecoration = false;
	traits->doubleBuffer = true;
	traits->sharedContext = 0;

	// graphic context
	osg::ref_ptr<osg::GraphicsContext> gc = osg::GraphicsContext::createGraphicsContext(traits.get());
	if(!gc.valid())
		return;
	gc->setClearColor(osg::Vec4f(0.0f,0.0f,0.0f,1.0f));
	gc->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// set viewer parameters
	m_windowSize = osg::Vec2(windowWidth, windowHeight);
	
	unsigned int viewportWidth = windowWidth / 2;

	//osg::ref_ptr<osgGA::TrackballManipulator> manip = new osgGA::TrackballManipulator;

	// left view
	m_leftView = new osgViewer::View;
	m_leftView->setName("left view");
	//m_leftView->setCameraManipulator(manip);
	addView(m_leftView);
	
	m_leftCamera = m_leftView->getCamera();
	m_leftCamera->setName("left cam");
	m_leftCamera->setViewport(new osg::Viewport(m_shiftWidth, 0, viewportWidth - 2 * m_shiftWidth, windowHeight));
	m_leftCamera->setGraphicsContext(gc.get());
		
	// right view
	m_rightView = new osgViewer::View;
	m_rightView->setName("right view");
	//m_leftView->setCameraManipulator(manip);
	addView(m_rightView);
	
	m_rightCamera = m_rightView->getCamera();
	m_rightCamera->setName("right cam");
	m_rightCamera->setViewport(new osg::Viewport(viewportWidth + m_shiftWidth, 0, viewportWidth - 2 * m_shiftWidth, windowHeight));
	m_rightCamera->setGraphicsContext(gc.get());

	// init vrpn
	m_VRPNClientManager.init();
	m_VRPNClientManager.setVerbose(false);
}

void CCaveViewer::setScene(osg::Node* scene)
{
	if(m_leftView && m_rightView)
	{
		m_sceneRotationPat = new osg::PositionAttitudeTransform;
		m_scenePositionPat = new osg::PositionAttitudeTransform;
		
		osg::Matrix m;
		m.makeScale(m_sceneScale);
		osg::ref_ptr<osg::MatrixTransform> tr = new osg::MatrixTransform;
		tr->setMatrix(m);

		m_sceneRotationPat->addChild(m_scenePositionPat);
		m_scenePositionPat->addChild(tr);
		//tr->addChild(scene);

		// only with OSG mesh
		osg::ref_ptr<osg::PositionAttitudeTransform> osgToOglSpace = new osg::PositionAttitudeTransform;
		osgToOglSpace->setAttitude(osg::Quat(-osg::PI_2, osg::Vec3(1,0,0)));
		tr->addChild(osgToOglSpace);
		osgToOglSpace->addChild(scene);

		m_leftView->setSceneData(m_sceneRotationPat);
		m_rightView->setSceneData(m_sceneRotationPat);

		// add shader to all drawables
		osg::Program* pgm = new osg::Program;
		pgm->setName("perPixelShader" );
		osg::Shader* vertShader = new osg::Shader(osg::Shader::VERTEX);
		bool b = vertShader->loadShaderSourceFromFile("perPixelLightning.vert");
		pgm->addShader(vertShader);
		osg::Shader* fragShader = new osg::Shader(osg::Shader::FRAGMENT);
		b = fragShader->loadShaderSourceFromFile("perPixelLightning.frag");
		pgm->addShader(fragShader);

		// nb lights uniforma
		osg::Uniform* uniform0 = new osg::Uniform("nbLights", 1);
		osg::Uniform* uniform1 = new osg::Uniform("baseMap", 0);
		
		CApplyShaderToTexturedGeodeVisitor vs(pgm);
		vs.addShaderUniform(uniform0);
		vs.addShaderUniform(uniform1);
		scene->accept(vs);

		// add light
		osg::Light *light = new osg::Light();
		light->setLightNum(0);
		// we set the light's position via a PositionAttitudeTransform object
		light->setPosition(osg::Vec4(m_lightPos.x(), m_lightPos.y(), m_lightPos.z(), 1.0));
		light->setDiffuse(osg::Vec4(1.0, 1.0, 1.0, 1.0));
		light->setSpecular(osg::Vec4(1.0, 1.0, 1.0, 1.0));
		light->setAmbient(osg::Vec4(1.0, 1.0, 1.0, 1.0));

		osg::StateSet* rootStateSet = m_sceneRotationPat->getOrCreateStateSet();
		rootStateSet->setMode(GL_LIGHTING, osg::StateAttribute::ON);
		rootStateSet->setMode(GL_LIGHT0, osg::StateAttribute::ON);

		osg::LightSource* lightSource = new osg::LightSource();
		lightSource->setLight(light);
		lightSource->setLocalStateSetModes(osg::StateAttribute::ON);
		lightSource->setStateSetModes(*rootStateSet, osg::StateAttribute::ON);
		m_sceneRotationPat->addChild(lightSource);

	}
}

void CCaveViewer::frame(double simulationTime)
{
	// run vrpn
	m_VRPNClientManager.run();

	// compute frustum
	// do not change camera position, only frustum adpatation
	double left, right, bottom, top, znear;
	double zfar = 10000.0;
	left = right = bottom = top = znear = 0.0;

	osg::Vec3 virtualLeftEye = -osg::Vec3(m_eyeDist / 2.0, 0, 0);
	osg::Vec3 virtualRightEye = osg::Vec3(m_eyeDist / 2.0, 0, 0);
	osg::Vec3 physicalLeftEye = -osg::Vec3(m_eyeDist / 2.0, 0, 0);
	osg::Vec3 physicalRightEye = osg::Vec3(m_eyeDist / 2.0, 0, 0);

	//osg::Vec3 flystickDirection;
	double thetaX = 0.0;
	double thetaY = 0.0;

	if(m_VRPNClientManager.getTrackerCount())
	{
		CVRPNClientManager::Tracker* tracker = m_VRPNClientManager.getTrackerFromId(CYCLOPE_ID);
		if(tracker)
		{
			// translation
			osg::Vec3 trackerPos = tracker->crtPos;

			// rotation
			osg::Matrix mRot(tracker->crtQuat);
			osg::Vec3 euler;
			MatrixToHpr(euler, mRot);
			double theta = euler.x();

			// update rotation
			osg::Quat q(osg::DegreesToRadians(theta), osg::Vec3(0,1,0));

			physicalLeftEye = q * physicalLeftEye;
			physicalRightEye = q * physicalRightEye;

			virtualLeftEye = q * virtualLeftEye;
			virtualRightEye = q * virtualRightEye;

			// update position
			physicalLeftEye += m_startEye;
			physicalRightEye += m_startEye;
			virtualLeftEye += m_startEye;
			virtualRightEye += m_startEye;

			physicalLeftEye.x() += trackerPos.x();
			physicalLeftEye.y() += trackerPos.z();
			physicalLeftEye.z() -= trackerPos.y();

			physicalRightEye.x() += trackerPos.x();
			physicalRightEye.y() += trackerPos.z();
			physicalRightEye.z() -= trackerPos.y();

			virtualLeftEye.x() += trackerPos.x();
			virtualLeftEye.y() += trackerPos.z();
			virtualLeftEye.z() -= trackerPos.y();

			virtualRightEye.x() += trackerPos.x();
			virtualRightEye.y() += trackerPos.z();
			virtualRightEye.z() -= trackerPos.y();
		}

		// flystick
		tracker = m_VRPNClientManager.getTrackerFromId(FLYSTICK_ID);
		if(tracker)
		{
			// get orientation
			osg::Matrix mRot(tracker->crtQuat);

			double angle;
			osg::Vec3 dir;
			tracker->crtQuat.getRotate(angle, dir);

			osg::Vec3 euler;
			MatrixToHpr(euler, mRot);
			//euler.z() = 0.0;//-euler.z();
			//euler.y() = 0.0;
			//HprToMatrix(mRot, euler);
			//std::cout << "euler x " << euler.x() << "euler y " << euler.y() << "euler z " << euler.z() << std::endl;
			//std::cout << "angle Q " << angle << ", dir Q " << dir.x() << ", " << dir.y() << ", " << dir.z() << std::endl;
			
			thetaX = euler.y();
			thetaY = euler.x();
		}
	}

	double analogY = m_VRPNClientManager.getAnalog(1);
	double analogX = m_VRPNClientManager.getAnalog(0);
	if(analogY)
	{
		// translate cube
		osg::Quat qX(osg::DegreesToRadians(thetaX), osg::Vec3(1,0,0));
		osg::Quat qY(osg::DegreesToRadians(thetaY), osg::Vec3(0,1,0));
		osg::Vec3 p = m_scenePositionPat->getPosition();
		
		osg::Vec3 flystickDirection = osg::Vec3(0,0,-1);
		flystickDirection = m_sceneRotationPat->getAttitude().inverse() * flystickDirection;
		flystickDirection = qX * flystickDirection;
		flystickDirection = qY * flystickDirection;
		p -= flystickDirection * 0.02 * analogY;
		
		m_scenePositionPat->setPosition(p);
	}
	if(analogX)
	{
		// rotate cube
		osg::Quat q = m_sceneRotationPat->getAttitude();
		osg::Quat qY(osg::DegreesToRadians(analogX), osg::Vec3(0,1,0));	// after PI/2 rotation on X axis, Z is up
		q = qY * q;
		m_sceneRotationPat->setAttitude(q);
	}

	osg::Vec3 dir;
	osg::Vec3 up;

	// set cameras position
	switch(m_caveFaceId)
	{
		case CAVE_CENTER:
		{
			dir = osg::Vec3(0,0,-1);
			up = osg::Vec3(0,1,0);

			// left eye
			left = -physicalLeftEye.x() - (m_caveWidth / 2.0);
			right = -physicalLeftEye.x() + (m_caveWidth / 2.0);
			bottom = -physicalLeftEye.y();
			top = m_caveHeight - physicalLeftEye.y();
			znear = (m_caveDepth / 2.0) + physicalLeftEye.z();

			m_leftCamera->setViewMatrixAsLookAt(virtualLeftEye, virtualLeftEye + dir, up);
			m_leftCamera->setProjectionMatrixAsFrustum(left, right, bottom, top, znear, zfar);
			//std::cout << "virtualLeftEye " << virtualLeftEye.x() << "," << virtualLeftEye.y() << "," << virtualLeftEye.z() << std::endl;
			//std::cout << "leftCamera : left right top bottom znear " << left << " " << right << " " << bottom << " " << top << " " << znear << std::endl;

			// right eye
			left = -physicalRightEye.x() - (m_caveWidth / 2.0);
			right = -physicalRightEye.x() + (m_caveWidth / 2.0);
			bottom = -physicalRightEye.y();
			top = m_caveHeight - physicalRightEye.y();
			znear = (m_caveDepth / 2.0) + physicalRightEye.z();

			m_rightCamera->setViewMatrixAsLookAt(virtualRightEye, virtualRightEye + dir, up);
			m_rightCamera->setProjectionMatrixAsFrustum(left, right, bottom, top, znear, zfar);
			//std::cout << "virtualRightEye " << virtualRightEye.x() << "," << virtualRightEye.y() << "," << virtualRightEye.z() << std::endl;
			//std::cout << "rightCamera : left right top bottom znear " << left << " " << right << " " << bottom << " " << top << " " << znear << std::endl;
			break;
		}
		case CAVE_RIGHT:
		{
			dir = osg::Vec3(1,0,0);
			up = osg::Vec3(0,1,0);

			// left eye
			left = -physicalLeftEye.z() - (m_caveDepth / 2.0);
			right = -physicalLeftEye.z() + (m_caveDepth / 2.0);
			bottom = -physicalLeftEye.y();
			top = m_caveHeight - physicalLeftEye.y();
			znear = (m_caveWidth / 2.0) - physicalLeftEye.x();

			m_leftCamera->setViewMatrixAsLookAt(virtualLeftEye, virtualLeftEye + dir, up);
			m_leftCamera->setProjectionMatrixAsFrustum(left, right, bottom, top, znear, zfar);
			//std::cout << "virtualLeftEye " << virtualLeftEye.x() << "," << virtualLeftEye.y() << "," << virtualLeftEye.z() << std::endl;
			//std::cout << "leftCamera : left right top bottom znear " << left << " " << right << " " << bottom << " " << top << " " << znear << std::endl;

			// right eye
			left = -physicalRightEye.z() - (m_caveDepth / 2.0);
			right = -physicalRightEye.z() + (m_caveDepth / 2.0);
			bottom = -physicalRightEye.y();
			top = m_caveHeight - physicalRightEye.y();
			znear = (m_caveWidth / 2.0) - physicalRightEye.x();

			m_rightCamera->setViewMatrixAsLookAt(virtualRightEye, virtualRightEye + dir, up);
			m_rightCamera->setProjectionMatrixAsFrustum(left, right, bottom, top, znear, zfar);
			//std::cout << "virtualRightEye " << virtualRightEye.x() << "," << virtualRightEye.y() << "," << virtualRightEye.z() << std::endl;
			//std::cout << "rightCamera : left right top bottom znear " << left << " " << right << " " << bottom << " " << top << " " << znear << std::endl;
			break;
		}
		case CAVE_LEFT:
		{
			dir = osg::Vec3(-1,0,0);
			up = osg::Vec3(0,1,0);

			// left eye
			left = physicalLeftEye.z() - (m_caveDepth / 2.0);
			right = physicalLeftEye.z() + (m_caveDepth / 2.0);
			bottom = -physicalLeftEye.y();
			top = m_caveHeight - physicalLeftEye.y();
			znear = (m_caveWidth / 2.0) + physicalLeftEye.x();

			m_leftCamera->setViewMatrixAsLookAt(virtualLeftEye, virtualLeftEye + dir, up);
			m_leftCamera->setProjectionMatrixAsFrustum(left, right, bottom, top, znear, zfar);
			//std::cout << "virtualLeftEye " << virtualLeftEye.x() << "," << virtualLeftEye.y() << "," << virtualLeftEye.z() << std::endl;
			//std::cout << "leftCamera : left right top bottom znear " << left << " " << right << " " << bottom << " " << top << " " << znear << std::endl;

			// right eye
			left = physicalRightEye.z() - (m_caveDepth / 2.0);
			right = physicalRightEye.z() + (m_caveDepth / 2.0);
			bottom = -physicalRightEye.y();
			top = m_caveHeight - physicalRightEye.y();
			znear = (m_caveWidth / 2.0) + physicalRightEye.x();

			m_rightCamera->setViewMatrixAsLookAt(virtualRightEye, virtualRightEye + dir, up);
			m_rightCamera->setProjectionMatrixAsFrustum(left, right, bottom, top, znear, zfar);
			//std::cout << "virtualRightEye " << virtualRightEye.x() << "," << virtualRightEye.y() << "," << virtualRightEye.z() << std::endl;
			//std::cout << "rightCamera : left right top bottom znear " << left << " " << right << " " << bottom << " " << top << " " << znear << std::endl;
			break;
		}
		case CAVE_BOTTOM:
		{
			dir = osg::Vec3(0,-1,0);
			up = osg::Vec3(0,0,1);

			// left eye
			left = -(m_caveWidth / 2.0) + physicalLeftEye.x();
			right = (m_caveWidth / 2.0) + physicalLeftEye.x();
			bottom = -(m_caveGroundBottom / 2.0) - physicalLeftEye.z();
			top = (m_caveGroundTop / 2.0) - physicalLeftEye.z();
			znear = physicalLeftEye.y();

			m_leftCamera->setViewMatrixAsLookAt(virtualLeftEye, virtualLeftEye + dir, up);
			m_leftCamera->setProjectionMatrixAsFrustum(left, right, bottom, top, znear, zfar);
			//std::cout << "virtualLeftEye " << virtualLeftEye.x() << "," << virtualLeftEye.y() << "," << virtualLeftEye.z() << std::endl;
			//std::cout << "leftCamera : left right top bottom znear " << left << " " << right << " " << bottom << " " << top << " " << znear << std::endl;

			// right eye
			left = -(m_caveWidth / 2.0) + physicalRightEye.x();
			right = (m_caveWidth / 2.0) + physicalRightEye.x();
			bottom = -(m_caveGroundBottom / 2.0) - physicalRightEye.z();
			top = (m_caveGroundTop / 2.0) - physicalRightEye.z();
			znear = physicalRightEye.y();

			m_rightCamera->setViewMatrixAsLookAt(virtualRightEye, virtualRightEye + dir, up);
			m_rightCamera->setProjectionMatrixAsFrustum(left, right, bottom, top, znear, zfar);
			//std::cout << "virtualRightEye " << virtualRightEye.x() << "," << virtualRightEye.y() << "," << virtualRightEye.z() << std::endl;
			//std::cout << "rightCamera : left right top bottom znear " << left << " " << right << " " << bottom << " " << top << " " << znear << std::endl;
			break;
		}
	}

	__super::frame(simulationTime);
}